package modes;

import java.io.File;
import lejos.nxt.Sound;
import main.ColorSensorChew;
import main.Engine;
import main.SensorEngine;
import main.SonicSensor;
import navigation.Direction;
import navigation.GoRightExitNavigator;
import navigation.IllegalMapPositionException;
import navigation.IllegalMapStateException;
import navigation.IllegalMoveException;
import navigation.Navigator;

/**
 * If Chewie reads green
 * @author Max
 */
public class ModeGreen extends AMode
{

    private final int MAX_COUNT_WHITE = 50;
    private int currentMaxWhite = MAX_COUNT_WHITE / 2;
    private int countWhite = 0;
    private int failureCount = 0;

    /**
     *
     * @param en Engine
     * @param se SensorEngine
     * @param cs ColorSensor
     * @param ss SonicSensor
     * @param navi Navigator
     */
    public ModeGreen(Engine en, SensorEngine se, ColorSensorChew cs, SonicSensor ss, Navigator navi)
    {
        super(en, se, cs, ss, navi);
    }

    /**
     * If Field is unkown, it will be added to the map.
     * Ask the navigator which direction shall be chosen
     * Chewie turns to direction
     * @post read black
     * @return new Mode
     * @throws InterruptedException
     *
     */
    public Mode run()
            throws InterruptedException
    {
        Mode mode = Mode.GREEN;
        engine.stop();

        if (isReadMoreThanOnce())
        {
            Sound.playSample(new File("chewy1.wav"), 100);
            ModeController.dm.echo("CrossRoad");
            moveOneStep();
            if (isCrossNotFree())
            {
                return Mode.FOUND_OBJECT;
            }

            if (!navi.fieldIsKnown())
            {
                Sound.beep();
                addField();
            }
            chooseDirection();
            do
            {
                color.readColor();
            } while ((!color.isColorGreen()) && (!color.isColorBlack()));
            turn();
            mode = decidedDirection(mode);
            rotationSensor.setRotating(true);
            mode = runTillBlack(mode);
            if (isAfterCrossNotFree())
            {
                mode = Mode.FOUND_OBJECT;
            }
        } else
        {
            mode = Mode.BLACK;
            actionGreen();
        }
        resetGreen();

        return mode;
    }

    private void actionGreen()
    {
        rotationSensor.setRotating(true);
        resetGreen();

        if (rotationSensor.isLeft())
        {
            engine.driveLeft(70);
        } else if (rotationSensor.isRight())
        {
            engine.driveRight(70);
        } else
        {
            engine.forward();
        }
    }

    private void turn()
            throws InterruptedException
    {
        try
        {
            if (navi.decideDirection() == Direction.RIGHT)
            {
                engine.setSpeed(600);
                Thread.sleep(800);
                engine.rotateRight();

                rotationSensor.setRotating(false);
                Thread.sleep(50);
            } else if (navi.decideDirection() == Direction.LEFT)
            {
                engine.setSpeed(600);
                Thread.sleep(800);
                engine.rotateLeft();
                rotationSensor.setRotating(false);
                Thread.sleep(50);
            } else if (navi.decideDirection() == Direction.BACKWARD)
            {
                rotationSensor.setRotating(false);
                engine.rotate180();
                engine.backward();
                Thread.sleep(500);
            } else
            {
                Thread.sleep(50);
                rotationSensor.setRotating(true);
            }
        } catch (IllegalMapStateException ex)
        {
            ModeController.dm.echo(ex.toString());
            engine.stop();
            Sound.playSample(new File("chewy1.wav"), 100);
            Thread.sleep(500);
            Sound.playSample(new File("chewy1.wav"), 100);

        }
        rotationSensor.rotateMiddle();
    }

    private void actionWhite()
            throws InterruptedException
    {
        if (rotationSensor.isMiddle())
        {
            rotationSensor.setRotating(false);
            if (countWhite == 0)
            {
                engine.switchRotation();
                failureCount++;
            } else if (countWhite >= currentMaxWhite)
            {
                engine.switchRotation();
                countWhite = 0;
                failureCount++;
                failureMode();
            }
            countWhite++;
        } else if (rotationSensor.isLeft())
        {
            engine.driveRight(70);
        } else
        {
            if (!rotationSensor.isRight())
            {
                return;
            }
            engine.driveLeft(70);
        }
    }

    private void failureMode()
    {
        if (failureCount > 2)
        {
            currentMaxWhite += 30;
        } else
        {
            currentMaxWhite = 100;
        }
    }

    private void resetGreen()
    {
        countWhite = 0;
        currentMaxWhite = 50;
        failureCount = 0;
    }

    private void chooseDirection()
            throws InterruptedException
    {
        engine.setSpeed(200);
        try
        {
            if (navi.decideDirection() == Direction.RIGHT)
            {
                rotationSensor.rotate(90);
            } else if (navi.decideDirection() == Direction.LEFT)
            {
                rotationSensor.rotate(-90);
            }
        } catch (IllegalMapStateException ex)
        {
            ModeController.dm.echo(ex.toString());
            engine.stop();
            Sound.playSample(new File("chewy1.wav"), 100);
            Thread.sleep(500);
            Sound.playSample(new File("chewy1.wav"), 100);

        }
        engine.forward();
    }

    private Mode findNewWay(Mode mode)
            throws InterruptedException
    {
        do
        {
            rotationSensor.rotateColorSensor();
            color.readColor();
            if (color.isColorGreen())
            {
                actionGreen();
            } else if (color.isColorBlack())
            {
                mode = Mode.BLACK;
                actionGreen();
            }

        } while ((!color.isColorGreen()) && (!color.isColorBlack()));
        return mode;
    }

    private void addField()
    {
        try
        {
            navi.addField(true, true, true, true);
        } catch (IllegalMapPositionException ex)
        {
            ModeController.dm.echo(ex.toString());

        }
    }

    private boolean isReadMoreThanOnce()
    {
        boolean readMoreThanOnce = true;
        for (int j = 0; j < 5; ++j)
        {
            color.readColor();
            if (color.isColorGreen())
            {
                continue;
            }
            readMoreThanOnce = false;
        }
        if (!readMoreThanOnce)
        {
            engine.setSpeed(50);
            engine.forward();
            readMoreThanOnce = true;
            for (int j = 0; j < 5; ++j)
            {
                color.readColor();
                if ((color.isColorGreen()) || (color.isColorWhite()))
                {
                    continue;
                }
                readMoreThanOnce = false;
            }
        }
        return readMoreThanOnce;
    }

    private Mode runTillBlack(Mode mode)
            throws InterruptedException
    {
        while (mode == Mode.GREEN)
        {

            color.readColor();
            rotationSensor.rotateColorSensor();

            if (color.isColorGreen())
            {
                actionGreen();
            } else if (color.isColorBlack())
            {
                engine.setSpeed(350);
                mode = Mode.BLACK;
                actionGreen();
            } else
            {
                if (!color.isColorWhite())
                {
                    continue;
                }
                actionWhite();
            }


        }
        return mode;
    }

    private void moveOneStep()
    {
        try
        {
            navi.moveOneStep();
        } catch (IllegalMoveException ex)
        {
            ModeController.dm.echo(ex.toString());
        }
    }

    private Mode decidedDirection(Mode mode) throws InterruptedException
    {
        try
        {
            if (navi.decideDirection() != Direction.FORWARD && navi.decideDirection() != Direction.BACKWARD)
            {
                mode = findNewWay(mode);
            }

            if (navi.decideDirection() == Direction.BACKWARD)
            {

                navi.directionChanged(Direction.BACKWARD);
            } else if (navi.decideDirection() == Direction.LEFT)
            {
                navi.directionChanged(Direction.LEFT);
            } else if (navi.decideDirection() == Direction.RIGHT)
            {
                navi.directionChanged(Direction.RIGHT);
            }
        } catch (IllegalMapStateException ex)
        {
            navi = new GoRightExitNavigator(navi);

        }
        return mode;
    }
}
